//
// Created by cch on 2022/5/1.
//

#include "imu_processer.h"
#include "base_type.h"

IMUGPS::ImuProcess::ImuProcess(const double acc_noise, const double gyro_noise,
                               const double acc_bias_noise,
                               const double gyro_bias_noise,
                               const Eigen::Vector3d &gravity)
    : acc_noise_(acc_noise), gyro_noise_(gyro_noise),
      acc_bias_noise_(acc_bias_noise), gyro_bias_noise_(gyro_bias_noise),
      gravity_(gravity) {}

void IMUGPS::ImuProcess::Process() {}

void IMUGPS::ImuProcess::Predict(const IMUGPS::ImuDataPtr &lastData,
                                 const IMUGPS::ImuDataPtr &curData,
                                 State *state) {
  //时间戳
  const double delta_t = curData->timestamp - lastData->timestamp;
  const double delta_t2 = delta_t * delta_t2;

  //根据龙科库塔积分计算标称状态姿态，只更新位置和速度，旋转暂不考虑
  Eigen::Matrix3d last_rotation = state->G_R_I;
  Eigen::Vector3d last_translation = state->G_P_I;
  Eigen::Vector3d last_velocity = state->G_V_I;

  Eigen::Vector3d k1_v_dot = last_rotation * lastData->acc + gravity_;
  Eigen::Vector3d k1_p_dot = last_velocity;

  Eigen::Vector3d k1_v = last_velocity + k1_v_dot * delta_t / 2;
  Eigen::Vector3d k2_v_dot = last_rotation * lastData->acc + gravity_;
  Eigen::Vector3d k2_p_dot = k1_v;

  Eigen::Vector3d k2_v = last_velocity + k2_v_dot * delta_t / 2;
  Eigen::Vector3d k3_v_dot = last_rotation * lastData->acc + gravity_;
  Eigen::Vector3d k3_p_dot = k2_v;

  Eigen::Vector3d k3_v = last_velocity + k3_v_dot * delta_t / 2;
  Eigen::Vector3d k4_v_dot = last_rotation * lastData->acc + gravity_;
  Eigen::Vector3d k4_p_dot = k3_v;

  state->G_V_I +=
      delta_t / 6 * (k1_v_dot + 2 * k2_v_dot + 2 * k3_v_dot + k4_v_dot);
  state->G_P_I +=
      delta_t / 6 * (k1_p_dot + 2 * k2_p_dot + 2 * k3_p_dot + k4_p_dot);

  Eigen::Vector3d delta_angle_axis =
      0.5 * (lastData->gyro + curData->gyro) - state->gyro_bias;

  state->G_R_I = state->G_R_I * Eigen::AngleAxisd(delta_angle_axis.norm(),
                                                  delta_angle_axis.normalized())
                                    .toRotationMatrix();

  //计算误差状态的协方差
  Eigen::Matrix<double, 15, 15> Fx = Eigen::Matrix<double, 15, 15>::Identity();
  Eigen::Matrix<double, 15, 12> Fi = Eigen::Matrix<double, 15, 12>::Zero();
  Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Identity();

  Fx.block<3, 3>(0, 0) = Eigen::Matrix<double, 3, 3>::Identity() * delta_t;
  Fx.block<3, 3>(3, 6) =
      state->G_R_I *
      IMUGPS::GetSkewMatrix(0.5 * (lastData->acc + curData->acc) -
                            state->acc_bias) *
      delta_t;
  Fx.block<3, 3>(3, 9) = -state->G_R_I * delta_t;
  Fx.block<3, 3>(6, 6) =
      Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized())
          .toRotationMatrix()
          .transpose();
  Fx.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity() * delta_t;

  Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();
  Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();
  Qi.block<3, 3>(6, 6) =
      delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();
  Qi.block<3, 3>(9, 9) =
      delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();
  Fi.block<12, 12>(0, 0) = Eigen::Matrix<double, 12, 12>::Identity();

  state->cov = Fx * state->cov * Fx.transpose() + Fi * Qi * Fi.transpose();
  state->timestamp = curData->timestamp;
  state->imuDataPtr = curData;
}